#include "modules/CtrlModule/PassivePlanner/SlipReactionPlanner.h"
#include "lib/WrapFuncs.h"
#include "data_struct/geometry/Point.h"
#include "data_struct/robot/Pose.h"
#include "lib/Time.h"
#include "data_struct/base/DataSet.h"
namespace behavior_controller
{
SlipReactionPlanner slip_reaction_planner;

void SlipReactionPlanner::run(double &linear_vel, double &angular_vel)
{

    if (DataSet::clean_mode == FOLLOW_MODE)
    {
        Back_Turn(M_PI_2,linear_vel, angular_vel);
    }
    else if (DataSet::clean_mode == NAVI_MODE)
    {
        Back_Forward_Speed_Up(linear_vel, angular_vel);
    }
}

void SlipReactionPlanner::Back_Turn(double turn_angle,double &linear_vel, double &angular_vel)
{
    double turn_time = 1;

    if (time_now < 1e-4) //如果时间为0,才会获取当前时间，以免下个周期到来时，时间变为最新时间
        time_now = Time::Now();
    if (Time::Now() - time_now < turn_time) //1s内后退
    {
        linear_vel = -0.25;
        angular_vel = 0;
        last_angle = DataSet::robot_pose.angle();
    }
    else  //第1s结束,旋转turn_angle
    {
        if(last_angle>= (M_PI-turn_angle))
        {
            if (DataSet::robot_pose.angle() > Funcs::NormalizeAngle(last_angle + turn_angle) &&  DataSet::robot_pose.angle()<0) // 转动 M_PI/4
            {
                finished = true;
                time_now = 0; //复位时间
            }
        }
        else
        {
            if (DataSet::robot_pose.angle() > Funcs::NormalizeAngle(last_angle + turn_angle))
            {
                finished = true;
                time_now = 0;
            }
        }
        if(!finished)
            Turn(linear_vel, angular_vel);
    }

}

void SlipReactionPlanner::Back_Forward_Speed_Up (double &linear_vel, double &angular_vel)
{
    double turn_time = 2;

    if (time_now < 1e-4) //如果时间为0,才会获取当前时间，以免下个周期到来时，时间变为最新时间
        time_now = Time::Now();
    if (Time::Now() - time_now < 1) //1s内后退
    {
        linear_vel = -0.25;
        angular_vel = 0;
    }
    else if ((Time::Now() - time_now > 1) && (Time::Now() - time_now < turn_time)) //第2s内前冲
    {
        linear_vel = 0.4;
        angular_vel = 0;
    }
    else if (Time::Now() - time_now > turn_time) //第3s结束
    {
        finished = true;
        time_now = 0; //复位掉时间
    }
}

void SlipReactionPlanner::Back(double &linear_vel, double &angular_vel)
{
    linear_vel = -0.25;
    angular_vel = 0;
}

void SlipReactionPlanner::Turn(double &linear_vel, double &angular_vel)
{
    linear_vel = 0;
    angular_vel = 0.5;
}

}
